#include "speed_prot.h"
#include "speed_cnrt.h"
#include "modbus.h"
#include "ms35711.h"
#include "xy_action.h"
#include <stdlib.h>
#include  "usart_device.h"
#include "z_action.h"
#include "config.h"
#include "gd32f30x_it.h"
#include "user_device.h"

#include "astar.h"

#ifndef IGNORE_Z
#define MAX_SLAVE_COUNT         3
#else
#define MAX_SLAVE_COUNT         0
#endif
#define SLAVE_ADDRESS(n)        (1 + (n))
#define PUMP_ADDRESS(n)         (rw_bit.independent_rinse ? (0x0D - 1 + (n)) : 0x0D)   // Z1对应清洗单元0x0D
#define Z_VALID_ADDR(addr)      (*(char*)slave_flag & (1 << ((addr)-1)))
#define Z_VALID_INDEX(i)        (*(char*)slave_flag & (1 << (i)))

bit pending_action = 0;         // 新动作
bit is_left_side;               // 左侧板, address = 0x0B
uchar rinse_state = 0;          // 进入清洗状态的针对应的位为1
bit no_pump_reversal = 0;       // 清洗完成不逆转

// 离开清洗状态
bit Do_Action_Leave_Rinse_State(void);


/////////////////////////////////////////////////////////////////////////////////
void Update_IO()
{
    ro_bit.x_at_home = X_AT_HOME;
    ro_bit.y_at_home = Y_AT_HOME;
}

// 读取清洗板报警状态
// 如果每根针有独立的清洗板, 则更新清洗液, 废液和压力报警;
// 如果共用清洗板, 则只更新清洗液和废液报警, 压力报警在清洗后决断.
void Update_Rinse_Alarm()
{
    uchar i;
    uchar n = rw_bit.independent_rinse ? 4 : 2;

    for(i = 1; i < MAX_SLAVE_COUNT; ++i)
    {
        if (!Z_VALID_INDEX(i))
            continue;

        if (Uart2_Read_RoBit(PUMP_ADDRESS(i), 0, n))
            Write_RO_bit(i*4, uart2_rx_buf[3], n);
    }
}


////////////////////////////////////////////////////////////////////////////////
// X电机指令
#define GPIOA_BASE  0x40010800
#define GPIOA_ODR   (GPIOA_BASE + 0x0C)
#define BitBand(Addr, BitNum)  ((unsigned long *)((Addr & 0xF0000000) + 0x2000000 + ((Addr & 0xFFFFF) << 5) + (BitNum << 2)))
#define PAout(n)  BitBand(GPIOA_ODR, n) //输出
// X电机脉冲输出
ulong* Aout2 = PAout(2);
void X_Step(uchar level)
{
//    *Aout2 = (ulong)level;
    if(level)
        gpio_bit_set(GPIOA,GPIO_PIN_2);
    else
        gpio_bit_reset(GPIOA,GPIO_PIN_2);
    srd_x.output = 0;
}

// X电机更新定时器,频率输出
void X_Set_Timer(ushort tick)
{
//    timer4_init.period = tick;
//    timer_init(TIMER4,&timer4_init);
    TIMER_CAR(TIMER4) = tick*2;
}
// X电机停止定时器
void X_Stop_Timer()
{
    timer_disable(TIMER4);
}

// 启动X电机
void Start_X_Motor()
{
    ro_word.X_DISTANCE =  0;
    IO_X_DIR(X_DIR_OUTPUT(srd_x.dir));
    IO_X_STEP(0);
    Spi0_ms35711X_write(0x00,0x0C0b);
    X_Set_Timer(FREQ_TABLE(srd_x.speed_index));
    turn_x_time = SystemTickHigh();
    
    timer_enable(TIMER4);
}

void Stop_X_Motor()
{
    Spi0_ms35711X_write(0x00,0x0C0a);
    
    
    IO_X_STEP(0);
    srd_x.output = 0;
    if (srd_x.dir == CCW)
            ro_word.X_DISTANCE = -srd_x.step_count;
    else
            ro_word.X_DISTANCE = srd_x.step_count;
    ro_word.X_DURATION = SystemTickHigh() - turn_x_time;
    srd_x.need_cleanup = 0;
    srd_x.running = 0;
}

// 执行复位指令时, 每一步调用一次
void X_Reset_Update()
{
    // 判定原点状态改变
    if (X_AT_HOME != srd_x.home_state)
    {
        srd_x.home_state = !srd_x.home_state;
        srd_x.hit_home = 1;
        ro_word.X_POS = 0;
        srd_x.step_left = X_RESET_BUFFER_STEP;
    }
    else
        ro_word.X_POS += srd_x.step_offset;
}

// 复位指令停止时调用
void X_Reset_Cleanup()
{
    if (!srd_x.hit_home)
        server_status = XY_STATUS_X_HOME_SENSOR_ERROR;

    Stop_X_Motor();
}

// 执行移动指令时, 每一步调用一次
void X_Move_To_Update()
{
    // 判定原点状态改变
    if (X_AT_HOME != srd_x.home_state)
    {
        srd_x.home_state = !srd_x.home_state;
        srd_x.hit_home = 1;
        ro_word.X_POS = 0;
        if (!srd_x.ignore_step_left)
        {
            srd_x.step_correction = labs(srd_x.target) - srd_x.step_left;
            srd_x.step_left = labs(srd_x.target);
        }
    }
    else
        ro_word.X_POS += srd_x.step_offset;
}

// 移动指令停止时调用
void X_Move_To_Cleanup()
{
    Stop_X_Motor();
}


////////////////////////////////////////////////////////////////////////////////
// Y电机指令
//#define GPIOB_BASE  0x40010C00
//#define GPIOB_ODR   (GPIOB_BASE + 0x0C)
//#define PBout(n)  BitBand(GPIOB_ODR, n) //输出
// Y电机脉冲输出
//ulong* Bout7 = PBout(7);
void Y_Step(uchar level)
{
    
    if(level)
        gpio_bit_set(GPIOB,GPIO_PIN_7);
    else
        gpio_bit_reset(GPIOB,GPIO_PIN_7);

    srd_y.output = 0;
}

// Y电机更新定时器
void Y_Set_Timer(ushort tick)
{
    timer3_init.period = tick;
    timer_init(TIMER3,&timer3_init);
}

// Y电机停止定时器
void Y_Stop_Timer()
{
   timer_disable(TIMER3);
}

// 启动Y电机
void Start_Y_Motor()
{
   ro_word.Y_DISTANCE = 0;

    IO_Y_DIR(Y_DIR_OUTPUT(srd_y.dir));
    IO_Y_STEP(0);
    Spi0_ms35711Y_write(0x00,0x0C0b);
    
    turn_y_time = SystemTickHigh();
    Y_Set_Timer(FREQ_TABLE(srd_y.speed_index));
    timer_enable(TIMER3);
}

// 停止Y电机
void Stop_Y_Motor()
{
    Spi0_ms35711Y_write(0x00,0x0C0a);
    
    
    IO_Y_STEP(0);
    srd_y.output = 0;
    if (srd_y.dir == CCW)
            ro_word.Y_DISTANCE = -srd_y.step_count;
    else
            ro_word.Y_DISTANCE = srd_y.step_count;
    ro_word.Y_DURATION = SystemTickHigh() - turn_y_time;
    srd_y.need_cleanup = 0;
    srd_y.running = 0;
}

// 执行复位指令时, 每一步调用一次
void Y_Reset_Update()
{ // 判定原点状态改变
    if (Y_AT_HOME != srd_y.home_state)
    {
        srd_y.home_state = !srd_y.home_state;
        srd_y.hit_home = 1;
        ro_word.Y_POS = 0;
        srd_y.step_left = Y_RESET_BUFFER_STEP;
    }
    else
        ro_word.Y_POS += srd_y.step_offset;
}

// 复位指令结束时调用
void Y_Reset_Cleanup()
{
    if (!srd_y.hit_home)
         server_status = XY_STATUS_Y_HOME_SENSOR_ERROR;
        

    Stop_Y_Motor();
}

// 执行移动指令时, 每一步调用一次
void Y_Move_To_Update()
{
    // 判定原点状态改变
    if (Y_AT_HOME != srd_y.home_state)
    {
        srd_y.home_state = !srd_y.home_state;
        srd_y.hit_home = 1;
        ro_word.Y_POS = 0;
        if (!srd_y.ignore_step_left)
        {
            srd_y.step_correction = labs(srd_y.target) - srd_y.step_left;
            srd_y.step_left = labs(srd_y.target);
        }
    }
    else
        ro_word.Y_POS += srd_y.step_offset;
}

// 移动指令结束时调用
void Y_Move_To_Cleanup()
{
    Stop_Y_Motor();
}

////////////////////////////////////////////////////////////////////////////////

void X_Init_Reset()
{
    srd_x.init = Start_X_Motor;
    srd_x.step = X_Step;
    srd_x.start_timer = X_Set_Timer;
    srd_x.stop_timer = X_Stop_Timer;
    srd_x.update = X_Reset_Update;
    srd_x.cleanup = X_Reset_Cleanup;
    srd_x.home_state = X_AT_HOME;
    srd_x.interp = 0;
}

void Y_Init_Reset()
{
    srd_y.init = Start_Y_Motor;
    srd_y.step = Y_Step;
    srd_y.start_timer = Y_Set_Timer;
    srd_y.stop_timer = Y_Stop_Timer;
    srd_y.update = Y_Reset_Update;
    srd_y.cleanup = Y_Reset_Cleanup;
    srd_y.home_state = Y_AT_HOME;
    srd_y.interp = 0;
}

void X_Init_Move(bit ignore_step_left)
{
    srd_x.init = Start_X_Motor;
    srd_x.step = X_Step;
    srd_x.start_timer = X_Set_Timer;
    srd_x.stop_timer = X_Stop_Timer;
    srd_x.update = X_Move_To_Update;
    srd_x.cleanup = X_Move_To_Cleanup;
    srd_x.home_state = X_AT_HOME;
    srd_x.interp = 0;
    srd_x.ignore_step_left = ignore_step_left;
}

void Y_Init_Move(bit ignore_step_left)
{
    srd_y.init = Start_Y_Motor;
    srd_y.step = Y_Step;
    srd_y.start_timer = Y_Set_Timer;
    srd_y.stop_timer = Y_Stop_Timer;
    srd_y.update = Y_Move_To_Update;
    srd_y.cleanup = Y_Move_To_Cleanup;
    srd_y.home_state = Y_AT_HOME;
    srd_y.interp = 0;
    srd_y.ignore_step_left = ignore_step_left;
}

// 移动到指定坐标, 成功返回1
bit Do_Move_XY_To(long x, long y, ushort xspeed, ushort yspeed)
{
    Y_Init_Move(0);
    srd_y.target = y;
    X_Init_Move(0);
    srd_x.target = x;

#if MOVE_METHOD == MOVE_METHOD_SEQUENCE
    speed_cntr_Move(&srd_y, y - ro_word.Y_POS, yspeed);
    while(srd_y.running)
        Process_Main_Loop();

    speed_cntr_Move(&srd_x, x - ro_word.X_POS, xspeed);
    while(srd_x.running)
        Process_Main_Loop();
#elif MOVE_METHOD == MOVE_METHOD_INDEPENDENT
    speed_cntr_Move(&srd_x, x - ro_word.X_POS, xspeed);
//    if(x == 0)
//        speed_cntr_Move(&srd_x,-ro_word.X_POS, xspeed);
    speed_cntr_Move(&srd_y, y - ro_word.Y_POS, yspeed);
//    if(x != 0)
//        speed_cntr_Move(&srd_x, -ro_word.X_POS + 133, xspeed);
//    speed_cntr_Move(&srd_x, x - ro_word.X_POS, xspeed);
    while(srd_x.running || srd_y.running)
        Process_Main_Loop();
#elif MOVE_METHOD == MOVE_METHOD_INTERP
    UNUSED(yspeed);

    speed_cntr_interp(x - x_pos, y - y_pos, xspeed);
    while(srd_x.running || srd_y.running)
        Process_Main_Loop();
#else
#error "Incorrect MOVE_METHOD value"
#endif

    return server_status == STATUS_IDLE;
}

/////////////////////////////////////////////////
typedef struct TimeData
{
    // 需要初始化的变量
    long pos;               // 起始位置, P1
    long target;            // 目标位置, P2
    long intermediate;      // 中间位置, P3
    ushort speed;           // 最高速度
    SpeedRampData *srd;

    // 计算过程
    ushort max_index;       // 最高速度索引
    long step;              // 全程
    long collision_step;    // 到碰撞的距离
    long collision_time;    // 到碰撞的时间
} TimeData;

// 运行指定步数所需的时间
long time_of_step(TimeData *d, long step)
{
    long left;

    // 加速阶段
    if (step <= d->max_index)
        return SUM_TABLE(step);
    else
    {
        left = d->step - step;

        // 匀速阶段
        if (left > d->max_index)
            return SUM_TABLE(d->max_index)
                    + TIME_TABLE(d->max_index) * (step - d->max_index);

        // 减速阶段, 全程时间 - 加速left距离的时间
        else
            return ( ((long)SUM_TABLE(d->max_index) << 1)
                    + TIME_TABLE(d->max_index) * (d->step - (d->max_index << 1))
                    )
                    - SUM_TABLE(left);
    }
}

ushort lower_bound(long *table, ushort count, ushort val)
{
    ushort first = 0;
    ushort last = count;
    ushort mid;
    while(first+1 < last)
    {
        mid = (first + last) >> 1;
        if (val <= table[mid])
            last = mid;
        else
            first = mid;
    }

    return first;
}

// 给定的时间所运行的步数
long step_of_time(TimeData *d, long time)
{
    long const_time;

    // 加速阶段
    if (time <= SUM_TABLE(d->max_index))
        return lower_bound(sum_table,
                           d->max_index >> SPEED_INTERVAL_CODE,
                           time) << SPEED_INTERVAL_CODE;

    // 匀速时间
    const_time = (d->step - (d->max_index << 1)) * TIME_TABLE(d->max_index);

    // 匀速阶段
    if (time <= const_time + SUM_TABLE(d->max_index))
        return d->max_index + (time - SUM_TABLE(d->max_index))
                / TIME_TABLE(d->max_index);

    // 减速阶段
    time = const_time + (SUM_TABLE(d->max_index) << 1) - time;
    if (time > 0)
        return d->step - (lower_bound(sum_table,
                                      d->max_index >> SPEED_INTERVAL_CODE,
                                      time) << SPEED_INTERVAL_CODE);

    return d->step;
}

// 初始化TimeData
void time_data_init(TimeData *d)
{
    d->max_index = speed_cntr_max_index(d->speed);
    d->step = labs(d->target - d->pos);

    // 最大索引不超过一半步数
    d->collision_step = d->step >> 1;
    if (d->max_index > d->collision_step)
        d->max_index = d->collision_step;

    d->collision_step = labs(d->intermediate - d->pos);
    d->collision_time = time_of_step(d, d->collision_step);
}

/*
  XY移动时中间不停顿的算法

  +------------------+
  |     Tx           |
  | P1 +-+           |
  |  Ty| |           |
  +----+-+ P3        |
  |//////|           |
  |/WALL/|           |
  |//////|   + P2    |
  +------+-----------+

  从P1运行到P2, 会撞到墙, 因此需要X(或Y)先启动, Y(或X)后启动.

  算法:
  1. 如果(X1, Y2)在WALL内, 碰撞方向u=Y, v=X;
     如果(X2, Y1)在WALL内, 碰撞方向u=X, v=Y;
     若都不在WALL内, 直接运行到P2, 结束
  2. 计算P1到P3的时间Tu, Tv.
  3. 若Tv <= Tu, 直接运行到P2, 结束.
  4. 令Dt = Tv - Tu
  5. 计算Dt时间内v方向运行的步数Sv
  6. V方向启动
  7. Sv步后, U启动
 */

// 移动到指定坐标, 并躲避障碍, 成功返回1
bit Move_XY_To(long x, long y, ushort xspeed, ushort yspeed)
{
//    TimeData time_x, time_y;
//    TimeData *u, *v;    // u: 碰撞轴; v: 不碰撞的轴
//    long dt;
//    if (Astar_Find_Path(ro_word.X_POS,ro_word.Y_POS, x, y))
//    {
//        if (astar_path.path_count == 1)
//        {
//            time_x.pos = ro_word.X_POS;
//            time_x.target = x;
//            time_x.intermediate = astar_path.path_x[0];
//            time_x.speed = xspeed;
//            time_x.srd = &srd_x;
//            time_data_init(&time_x);
//            time_y.pos = ro_word.Y_POS;
//            time_y.target = y;
//            time_y.intermediate = astar_path.path_y[0];
//            time_y.speed = yspeed;
//            time_y.srd = &srd_y;
//            time_data_init(&time_y);
//            // 碰撞方向
//            if (Astar_Move_X_First())
//            {
//                // Y方向碰撞
//                u = &time_y;
//                v = &time_x;
//            }
//            else
//            {
//                // X方向碰撞
//                u = &time_x;
//                v = &time_y;
//            }

//            // 驶出碰撞区与碰撞的时间差
//            dt = v->collision_time - u->collision_time;

//            // 碰撞前已经驶出碰撞区
//            if (dt <= 0)
//            {
//                return Do_Move_XY_To(x, y, xspeed, yspeed);
//            }
//                

//            // dt时间驶过的步数
//            dt = step_of_time(v, dt);
//            // 移动初始化
//            Y_Init_Move(0);
//            srd_y.target = y;
//            X_Init_Move(0);
//            srd_x.target = x;

//            // 启动不碰撞的轴, 并等待距离超过dt
//            speed_cntr_Move(v->srd, v->target - v->pos, v->speed);
//            while(v->srd->running && v->srd->step_count < dt)
//                Process_Main_Loop();

//            // 如果出错, 返回
//            if (server_status != STATUS_IDLE)
//                return 0;

//            // 启动碰撞轴
//            speed_cntr_Move(u->srd, u->target - u->pos, u->speed);
//            while(srd_x.running || srd_y.running)
//                Process_Main_Loop();

//            return server_status == STATUS_IDLE;
//        }
//        for(astar_path.path_index = 0;
//            astar_path.path_index < astar_path.path_count;
//            ++astar_path.path_index)
//        {
//            if (!Do_Move_XY_To(astar_path.path_x[astar_path.path_index],
//                               astar_path.path_y[astar_path.path_index],
//                               xspeed, yspeed))
//                return 0;
//        }
//    }
    return Do_Move_XY_To(x, y, xspeed, yspeed);
}

///////////////////////////////////////////////////////////////////////
void Slave_Communication_Error(uchar slave)
{
    server_status = STATUS_COMMUNICATION_ERROR(slave);
}

void Slave_Status_Error(uchar slave, ushort status)
{
    if ((slave == SLAVE_ADDRESS(1) || slave == SLAVE_ADDRESS(2))
            && status == Z_STATUS_INSUFFICIENT_REAGENT)
    {
        server_status = XY_STATUS_INSUFFICIENT_REAGENT;
    }
    else
        server_status = ((ushort)slave << 8) | status;
}

// Z发送回零指令, 如果已经在零位, 不执行任何动作.
// 该函数只发送指令, 不等待完成
bit Slave_Z_Home(uchar slave)
{
    // 发送复位指令
    if (Z_VALID_ADDR(slave))
    {
        Uart2_Begin_Write_Word(slave, 0x09, 1);
        UART2_APPEND2(2, Z_ACTION_Z_HOME);
        UART2_END();

        if (!Uart2_Exchange_Message())
        {
            Slave_Communication_Error(slave);
            return 0;
        }
    }
    return 1;
}

// 等待从机空闲, 成功返回1; 否则返回0.
bit Slave_Wait_Idle(uchar slave)
{
    if (slave != PUMP_ADDRESS(1) && slave != PUMP_ADDRESS(2) && !Z_VALID_ADDR(slave))
        return 1;

    while(1)
    {
            
        if (!Uart2_Read_Event_Count(slave))
        {
            Slave_Communication_Error(slave);
            return 0;
        }
        switch(*(ushort*)&uart2_rx_buf[2])
        {
    
                    case STATUS_IDLE:
            return 1;
    
                    case STATUS_BUSY:
            continue;

        default:
#ifdef IGNORE_PICKUP_ERROR
            // 忽略抓杯传感器错误
            if (slave == SLAVE_ADDRESS(0) && Z_STATUS_PICK_CUP_SENSOR_ERROR == *(ushort*)&uart1_rx_buf[2])
                return 1;
#endif//IGNORE_PICKUP_ERROR

            Slave_Status_Error(slave, *(ushort*)&uart2_rx_buf[2]);
            return 0;
        }
    }
}

// 将Z0/Z1/Z2回到零位, 成功返回1
bit Slave_Home()
{
    uchar i;

    // 发送Z回零指令
    for(i=2; i<MAX_SLAVE_COUNT; ++i)
    {
        if (!Slave_Z_Home(SLAVE_ADDRESS(i)))
            return 0;
    }

    // 等待Z复位完成
    for(i=2; i<MAX_SLAVE_COUNT; ++i)
    {
        if (!Slave_Wait_Idle(SLAVE_ADDRESS(i)))
            return 0;
    }

    return 1;
}

#define MAX_HEAT_TICK_HIGH  10000

// 等待针温度稳定
bit Slave_Wait_Heat(uchar slave)
{
    uint32_t here_time;
		here_time = 0;

    if (!Z_VALID_ADDR(slave))
        return 1;

    for(here_time = SystemTickHigh(); SystemTickHigh() - here_time < MAX_HEAT_TICK_HIGH; )
    {
        if (!Uart2_Read_RoBit(slave, 6, 1))
        {
            Slave_Communication_Error(slave);
            return 0;
        }

        if (uart2_rx_buf[3] & 1)
            break;
    }

    return 1;
}


// 将Z移动到指定位置
bit Move_XYZ_To(uchar slave)
{
    // 所有Z回零: 如果XY不在目标位置或者需要探测液面
    if (rw_word.X_TARGET_1 != ro_word.X_POS || rw_word.Y_TARGET_1 != ro_word.Y_POS)
    {
        if (!Slave_Home())
            return 0;
    }

    // XY移动到拾取/释放测试杯位置
    if (!Move_XY_To(rw_word.X_TARGET_1,
                    rw_word.Y_TARGET_1,
                    rw_word.XMAX_SPEED,
                    rw_word.YMAX_SPEED))
        return 0;

#ifndef IGNORE_Z
    // Z移动到目标位置
    Uart2_Begin_Write_Word(slave, 0x09, 3);
    UART2_APPEND2(2, Z_ACTION_Z_MOVE_TO);
    UART2_APPEND2(4,   rw_word.Z_TARGET_1);
    UART2_END();

    if (!Uart2_Exchange_Message())
    {
        Slave_Communication_Error(slave);
        return 0;
    }
    if (!Slave_Wait_Idle(slave))
        return 0;
#endif//IGNORE_Z

    return 1;
}
///////////////////////////////////////////

void Reset_X()
{
    X_Init_Reset();
    srd_x.target = srd_x.home_state ? X_RESET_BUFFER_STEP : -X_RESET_BUFFER_STEP;
    speed_cntr_Move(&srd_x, srd_x.home_state ? rw_word.X_MAX_DISTANCE : -rw_word.X_MAX_DISTANCE, rw_word.XMAX_SPEED);

    // 等待X轴完成
    while(srd_x.running)
        Process_Main_Loop();
}

void Reset_Y()
{
    Y_Init_Reset();
    srd_y.target = srd_y.home_state ? Y_RESET_BUFFER_STEP : -Y_RESET_BUFFER_STEP;
    speed_cntr_Move(&srd_y, srd_y.home_state ? rw_word.Y_MAX_DISTANCE :-from_little_endian_32(rw_word.Y_MAX_DISTANCE), rw_word.YMAX_SPEED);

    // 等待Y轴完成
    while(srd_y.running)
        Process_Main_Loop();
}

//////////////////////////////////////////////////////////
void Do_Action_Stop()
{
    uchar i;

    // 停止X电机
    if (srd_x.running)
    {
        X_Stop_Timer();
        Stop_X_Motor();
    }

    // 停止Y电机
    if (srd_y.running)
    {
        Y_Stop_Timer();
        Stop_Y_Motor();
    }

    // 发送STOP指令给Z
    for(i=0; i<MAX_SLAVE_COUNT; ++i)
    {
        if (!Z_VALID_INDEX(i))
            continue;

        Uart2_Begin_Write_Word(SLAVE_ADDRESS(i), 0x09, 1);
        UART2_APPEND2(2, Z_ACTION_STOP);
        UART2_END();

        if (!Uart2_Exchange_Message())
        {
            Slave_Communication_Error(SLAVE_ADDRESS(i));
            return;
        }
    }

    // 等待Z停止
    for(i=0; i<MAX_SLAVE_COUNT; ++i)
    {
        if (!Z_VALID_INDEX(i))
            continue;

        // 此处不必检查通讯状态
        Slave_Wait_Idle(SLAVE_ADDRESS(i));
    }

    server_status = STATUS_IDLE;
}


void Do_Action_Reset()
{
    uchar i;

    // 不复位注射器, 离开清洗状态; 若复位注射器,此动作多余
    if (rw_word.REAGENT_VOLUME)
    {
        if (!Do_Action_Leave_Rinse_State())
            return;
    }

    // 发送复位Z指令
    for(i=0; i<MAX_SLAVE_COUNT; ++i)
    {
        if (!Z_VALID_INDEX(i))
            continue;

        Uart2_Begin_Write_Word(SLAVE_ADDRESS(i), 0x09, 1);
        UART2_APPEND2(2, Z_ACTION_Z_RESET);
        UART2_END();

        if (!Uart2_Exchange_Message())
        {
            Slave_Communication_Error(SLAVE_ADDRESS(i));
            return;
        }
    }

    // 等待Z复位完成
    for(i=0; i<MAX_SLAVE_COUNT; ++i)
    {
        if (!Slave_Wait_Idle(SLAVE_ADDRESS(i)))
            return;
    }

    server_status = STATUS_IDLE;

    // 左侧臂复位时先复位Y轴, 然后再复位X轴, 以免与测试杯盘相撞;
    // 右侧臂先复位X轴, 再Y轴, 以免与抓手相撞.
    if (is_left_side)
    {
        // 复位Y轴
        Reset_Y();

        // 判断Y复位状态
        if (server_status != STATUS_IDLE)
            return;

        // 复位X轴
        Reset_X();

        // 判断X复位状态
        if (server_status != STATUS_IDLE)
            return;
    }
    else
    {
        // 复位X轴
        Reset_X();

        // 判断X复位状态
        if (server_status != STATUS_IDLE)
            return;

        // 复位Y轴
        Reset_Y();

        // 判断Y复位状态
        if (server_status != STATUS_IDLE)
            return;
    }

    // 不复位注射器, 返回
    if (rw_word.REAGENT_VOLUME)
        return;

    //
    // 移动到清洗位再复位注射器, 以防止水从针头滴出
    //

    // 共用清洗泵, 各针单独清洗
    if (!rw_bit.independent_rinse)
    {
        for(i=1; i<MAX_SLAVE_COUNT; ++i)
        {
            if (!Z_VALID_INDEX(i))
                continue;

            // 移动到target_1处
            if (!Move_XY_To(i == 1 ? rw_word.X_TARGET_1 : rw_word.X_TARGET_2,
                            i == 1 ? rw_word.Y_TARGET_1 : rw_word.Y_TARGET_2,
                            rw_word.XMAX_SPEED, rw_word.YMAX_SPEED))
                return;

            // 将Z下降到指定高度
            Uart2_Begin_Write_Word(SLAVE_ADDRESS(i), 0x09, 3);
            UART2_APPEND2(2,  Z_ACTION_Z_MOVE_TO);
            UART2_APPEND2(4, i == 1 ? rw_word.Z_TARGET_1 : rw_word.Z_TARGET_2);
            UART2_END();

            if (!Uart2_Exchange_Message())
            {
                Slave_Communication_Error(SLAVE_ADDRESS(i));
                return;
            }

            // 等待Z移动完成
            if (!Slave_Wait_Idle(SLAVE_ADDRESS(i)))
                return;

            // 发送复位注射器指令
            Uart2_Begin_Write_Word(SLAVE_ADDRESS(i), 0x09, 1);
            UART2_APPEND2(2,  Z_ACTION_S_RESET);
            UART2_END();

            if (!Uart2_Exchange_Message())
            {
                Slave_Communication_Error(SLAVE_ADDRESS(i));
                return;
            }

            // 等待注射器复位完成
            if (!Slave_Wait_Idle(SLAVE_ADDRESS(i)))
                return;

            // Z复位
            if (!Slave_Home())
                return;
        }
    }
    else    // 单独清洗泵, 同时清洗
    {
        // 移动到target_1处
        if (!Move_XY_To(rw_word.X_TARGET_1, rw_word.Y_TARGET_1, rw_word.XMAX_SPEED, rw_word.YMAX_SPEED))
            return;

        // 将Z下降到指定高度
        for(i=1; i<MAX_SLAVE_COUNT; ++i)
        {
            if (!Z_VALID_INDEX(i))
                continue;

            Uart2_Begin_Write_Word(SLAVE_ADDRESS(i), 0x09, 3);
            UART2_APPEND2(2,  Z_ACTION_Z_MOVE_TO);
            UART2_APPEND2(4, i == 1 ? rw_word.Z_TARGET_1 : rw_word.Z_TARGET_2);
            UART2_END();

            if (!Uart2_Exchange_Message())
            {
                Slave_Communication_Error(SLAVE_ADDRESS(i));
                return;
            }
        }

        // 等待Z移动完成
        for(i=1; i<MAX_SLAVE_COUNT; ++i)
        {
            if (!Z_VALID_INDEX(i))
                continue;

            if (!Slave_Wait_Idle(SLAVE_ADDRESS(i)))
                return;
        }

        // 发送复位注射器指令
        for(i=1; i<MAX_SLAVE_COUNT; ++i)
        {
            if (!Z_VALID_INDEX(i))
                continue;

            Uart2_Begin_Write_Word(SLAVE_ADDRESS(i), 0x09, 1);
            UART2_APPEND2(2,  Z_ACTION_S_RESET);
            UART2_END();

            if (!Uart2_Exchange_Message())
            {
                Slave_Communication_Error(SLAVE_ADDRESS(i));
                return;
            }
        }

        // 等待注射器复位完成
        for(i=1; i<MAX_SLAVE_COUNT; ++i)
        {
            if (!Z_VALID_INDEX(i))
                continue;

            if (!Slave_Wait_Idle(SLAVE_ADDRESS(i)))
                return;
        }

        // Z复位
        if (!Slave_Home())
            return;
    }

    server_status = STATUS_IDLE;
}

#define PICKUP_CUP_EXTRA_STEP   ((short)(PICKUP_CUP_EXTRA_OFFSET * SPR / Y_PERIMETER))

// 拾取(pick=1)/释放(pick=0)测试杯
void Do_Action_Pick_Cup(bit pick)
{
    // 离开清洗状态
    if (!Do_Action_Leave_Rinse_State())
        return;

    // 所有Z回零
    if (!Slave_Home())
        return;

    // XY移动到拾取/释放测试杯位置
    if (!Move_XY_To(rw_word.X_TARGET_1,
                    pick ? rw_word.Y_TARGET_1 - rw_word.PICKUP_CUP_DISTANCE : rw_word.Y_TARGET_1,
                    rw_word.XMAX_SPEED,
                    rw_word.YMAX_SPEED))
        return;

#ifndef IGNORE_Z
    // 给Z0发送z_pickup_cup指令
    Uart2_Begin_Write_Word(SLAVE_ADDRESS(0), 0x09, 3);
    UART2_APPEND2(2, Z_ACTION_PICK_CUP);
    UART2_APPEND2(4,   rw_word.Z_TARGET_1);
    UART2_END();
    if (!Uart2_Exchange_Message())
    {
        Slave_Communication_Error(SLAVE_ADDRESS(0));
        return;
    }
    if (!Slave_Wait_Idle(SLAVE_ADDRESS(0)))
        return;
#endif// IGNORE_Z

#if PICKUP_CUP_DELAY
    // 放杯时, 延迟一会儿再平移
    if (!pick)
        Delay_ms(PICKUP_CUP_DELAY);
#endif// PICKUP_CUP_DELAY

    // XY移动到拾取/释放测试杯偏移位置
    if (!Move_XY_To(rw_word.X_TARGET_1,
                    pick ? rw_word.Y_TARGET_1 + PICKUP_CUP_EXTRA_STEP : rw_word.Y_TARGET_1 - rw_word.PICKUP_CUP_DISTANCE,
                    rw_word.XMAX_SPEED,
                    rw_word.YMAX_SPEED))
        return;

#if PICKUP_CUP_DELAY
    // 抓杯时, 延迟一会儿再上移
    if (pick)
        Delay_ms(PICKUP_CUP_DELAY);
#endif// PICKUP_CUP_DELAY

#ifndef IGNORE_Z
    // 机械手回零
    if (!Slave_Z_Home(SLAVE_ADDRESS(0)) || !Slave_Wait_Idle(SLAVE_ADDRESS(0)))
        return;

    // 检查HOLDING_CUP状态
    if (!Uart2_Read_RoBit(SLAVE_ADDRESS(0), 0x04, 1))
    {
        Slave_Communication_Error(SLAVE_ADDRESS(0));
        return;
    }

    // 拾取/释放测试杯时, 如果机械手没有/有测试杯, 报错
    if (pick)
    {
        if ((uart2_rx_buf[3] & 0x01) == 0)
            server_status = XY_STATUS_PICKUP_CUP_ERROR;
    }
    else
    {
        if (((uart2_rx_buf[3] & 0x01)) == 1)
            server_status = XY_STATUS_RELEASE_CUP_ERROR;
    }
#endif// IGNORE_Z
}

void Do_Action_Aspirate(uchar slave, bit waitHeat)
{
#ifdef IGNORE_Z
    UNUSED(slave);
#endif// IGNORE_Z

    // 离开清洗状态
    if (!Do_Action_Leave_Rinse_State())
        return;

    // 所有Z回零: 如果XY不在目标位置或者需要探测液面
    if (rw_word.X_TARGET_1 != ro_word.X_POS || rw_word.Y_TARGET_1 != ro_word.Y_POS
            || (!rw_bit.ignore_level_sensor
                && rw_word.REAGENT_VOLUME >= 0
                && (rw_word.REAGENT_BOTTLE_TYPE & REAGENT_FLAG_IGNORE_PROBE_ERROR) == 0) )
    {
        if (!Slave_Home())
            return;
    }

    // XY移动到拾取/释放测试杯位置
    if (!Move_XY_To(rw_word.X_TARGET_1,
                    rw_word.Y_TARGET_1,
                    rw_word.XMAX_SPEED,
                    rw_word.YMAX_SPEED))
        return;

#ifndef IGNORE_Z
    if (waitHeat && !Slave_Wait_Heat(slave))
        return;

    if (rw_word.REAGENT_VOLUME >= 0)
    {
        // 吸样

        // 给Z发送PROBE_AND_ASPIRATE指令
        Uart2_Begin_Write_Word(slave, 0x09, 5);
        UART2_APPEND2(2, Z_ACTION_PROBE_AND_ASPIRATE);
        UART2_APPEND2(4,   rw_word.Z_TARGET_1);
        UART2_APPEND2(3,  rw_word.REAGENT_VOLUME);
        UART2_APPEND2(2, rw_word.REAGENT_BOTTLE_TYPE
                     | (rw_bit.ignore_level_sensor ? REAGENT_FLAG_IGNORE_PROBE_ERROR : 0)
                     | (slave == SLAVE_ADDRESS(0) ? REAGENT_FLAG_HOLD_POSITION : 0));
        UART2_END();

        if (!Uart2_Exchange_Message())
        {
            Slave_Communication_Error(slave);
            return;
        }

        if (!Slave_Wait_Idle(slave))
            return;
    }
    else
    {
        // 加样

        // Z移动到目标位置
        Uart2_Begin_Write_Word(slave, 0x09, 3);
        UART2_APPEND2(2, Z_ACTION_Z_MOVE_TO);
        UART2_APPEND2(4,   rw_word.Z_TARGET_1);
        UART2_END();

        if (!Uart2_Exchange_Message())
        {
            Slave_Communication_Error(slave);
            return;
        }
        if (!Slave_Wait_Idle(slave))
            return;

        // 加样
        if (slave != SLAVE_ADDRESS(0))
        {
            Uart2_Begin_Write_Word(slave, 0x09, 5);
            UART2_APPEND2(2, Z_ACTION_ASPIRATE);
            UART2_APPEND2(4,   rw_word.Z_TARGET_1);
            UART2_APPEND2(3,  rw_word.REAGENT_VOLUME);
            UART2_APPEND2(2, rw_word.REAGENT_BOTTLE_TYPE | REAGENT_FLAG_NO_DELAY);
            UART2_END();

            if (!Uart2_Exchange_Message())
            {
                Slave_Communication_Error(slave);
                return;
            }
            if (!Slave_Wait_Idle(slave))
                return;
        }
    }

    // 读取液面位置
    if (!Uart2_Read_RoWord(slave, slave == SLAVE_ADDRESS(0) ? 0x07 : 0x09, 2))
    {
        Slave_Communication_Error(slave);
        return;
    }
    rw_word.Z_TARGET_1 = *(long*)&uart2_rx_buf[3];

    // 读取试剂剩余量
    if (rw_word.REAGENT_VOLUME >= 0 && slave != SLAVE_ADDRESS(0))
    {
        if (!Uart2_Read_RoWord(slave, 0x12, 1))
        {
            Slave_Communication_Error(slave);
            return;
        }

        ro_word.REAGENT_RESIDUAL_VOLUME = *(ushort*)&uart2_rx_buf[3];
    }

    // 机械手向上走, 直到检测不到传感器值
    if (slave == SLAVE_ADDRESS(0) && !(rw_word.REAGENT_BOTTLE_TYPE & REAGENT_FLAG_HOLD_POSITION))
    {
        // Z_ACTION_Z_RESET
        Uart2_Begin_Write_Word(slave, 0x09, 1);
        UART2_APPEND2(2, Z_ACTION_Z_RESET);
        UART2_END();

        if (!Uart2_Exchange_Message())
        {
            Slave_Communication_Error(slave);
            return;
        }

        while(1)
        {
            if (!Uart2_Read_RoBit(slave, 5, 1))
            {
                Slave_Communication_Error(slave);
                return;
            }

            if (uart2_rx_buf[3] & 0x01)
            {
                if (!Uart2_Read_RoWord(slave, 0, 2))
                {
                    Slave_Communication_Error(slave);
                    return;
                }

                rw_word.Z_TARGET_1 += *(long*)&uart2_rx_buf[3];
                rw_word.Z_TARGET_1 >>= 1;
                break;
            }

            if (!Uart2_Read_Event_Count(slave))
            {
                Slave_Communication_Error(slave);
                return;
            }

            if (*(ushort*)&uart2_rx_buf[2] != STATUS_BUSY)
                break;
        }

        Slave_Wait_Idle(slave);
    }
#endif//IGNORE_Z
}

// 离开清洗状态
bit Do_Action_Leave_Rinse_State()
{
    uchar i;
    uchar mask = rinse_state;

    mask &= *(char*)slave_flag;
    if (mask == 0)
        return 1;

    // 所有Z回零
    if (!Slave_Home())
        return 0;

    // 注射器回到吸样位置
    for(i = 1; i < MAX_SLAVE_COUNT; ++i)
    {
        if ((mask & (1 << i)) == 0)
            continue;

        Uart2_Begin_Write_Word(SLAVE_ADDRESS(i), 0x09, 4);
        UART2_APPEND2(2, Z_ACTION_S_MOVE_TO);
        UART2_APPEND2(4,   0);
        UART2_APPEND2(2,  SYRINGE_AIR_VOLUME);
        UART2_END();

        if (!Uart2_Exchange_Message())
        {
            Slave_Communication_Error(SLAVE_ADDRESS(i));
            return 0;
        }
    }

    // 等待Z完成
    for(i = 1; i < MAX_SLAVE_COUNT; ++i)
    {
        if ((mask & (1 << i)) == 0)
            continue;

        if (!Slave_Wait_Idle(SLAVE_ADDRESS(i)))
            return 0;
    }

    if(!no_pump_reversal)
    {
        // 启动蠕动泵逆转泄压
        for(i = 1; i < MAX_SLAVE_COUNT; ++i)
        {
            if ((mask & (1 << i)) == 0)
                continue;

            Uart2_Begin_Write_Word(PUMP_ADDRESS(i), 0x05, 1);
            UART2_APPEND2(2, 2);
            UART2_END();
            Uart2_Exchange_Message();

            // 只有一个清洗泵, 发一次命令就可以了
            if (!rw_bit.independent_rinse)
                break;
        }
    }

    // 清除清洗状态
    rinse_state &= ~mask;

    return 1;
}

// 进入清洗状态, mask相应位为1的针动作
void Do_Action_Rinse(uchar mask)
{
    uchar i;
    ushort here_time;

    // 离开清洗状态
    if (!Do_Action_Leave_Rinse_State())
        return;

    // 所有Z回零
    if (!Slave_Home())
        return;

    if (!rw_bit.no_rinsing_home || !rw_bit.no_rinsing_home_x || !rw_bit.no_rinsing_home_y)
    {
        // XY移动清洗位置
        if (!Move_XY_To((!rw_bit.no_rinsing_home || !rw_bit.no_rinsing_home_x) && rw_word.X_TARGET_1 >= 0 ? -X_RESET_BUFFER_STEP : rw_word.X_TARGET_1,
                        (!rw_bit.no_rinsing_home || !rw_bit.no_rinsing_home_y) && rw_word.Y_TARGET_1 >= 0 ? -Y_RESET_BUFFER_STEP : rw_word.Y_TARGET_1,
                        rw_word.XMAX_SPEED,
                        rw_word.YMAX_SPEED))
            return;

        //
        // 如果X或Y未经过原点, 复位一次相应轴
        //

        // 复位Y轴
        if (!Y_AT_HOME && (!rw_bit.no_rinsing_home || !rw_bit.no_rinsing_home_y))
        {
            Y_Init_Reset();
            srd_y.target = srd_y.home_state ? Y_RESET_BUFFER_STEP : -Y_RESET_BUFFER_STEP;
            speed_cntr_Move(&srd_y, srd_y.home_state ? rw_word.Y_MAX_DISTANCE : -rw_word.Y_MAX_DISTANCE, rw_word.YMAX_SPEED);
        }

        // 复位X轴
        if (!X_AT_HOME && (!rw_bit.no_rinsing_home || !rw_bit.no_rinsing_home_x))
        {
            X_Init_Reset();
            srd_x.target = srd_x.home_state ? X_RESET_BUFFER_STEP : -X_RESET_BUFFER_STEP;
            speed_cntr_Move(&srd_x, srd_x.home_state ? rw_word.X_MAX_DISTANCE : -rw_word.X_MAX_DISTANCE, rw_word.XMAX_SPEED);
        }

        // 等待XY轴完成
        while(srd_x.running || srd_y.running)
            Process_Main_Loop();
        if (server_status != STATUS_IDLE)
            return;
    }

    // XY移动清洗位置
    if (!Move_XY_To(rw_word.X_TARGET_1,
                    rw_word.Y_TARGET_1,
                    rw_word.XMAX_SPEED,
                    rw_word.YMAX_SPEED))
        return;

    mask &= *(uchar*)slave_flag;

    // 将Z移动到清洗位
    for(i = 1; i < MAX_SLAVE_COUNT; ++i)
    {
        if ((mask & (1 << i)) == 0)
            continue;

        Uart2_Begin_Write_Word(SLAVE_ADDRESS(i), 0x09, 3);
        UART2_APPEND2(2, Z_ACTION_Z_MOVE_TO);
        UART2_APPEND2(4,   rw_word.Z_TARGET_1);
        UART2_END();

        if (!Uart2_Exchange_Message())
        {
            Slave_Communication_Error(SLAVE_ADDRESS(i));
            return;
        }
    }

    // 等待Z移动完成
    for(i = 1; i < MAX_SLAVE_COUNT; ++i)
    {
        if ((mask & (1 << i)) == 0)
            continue;

        if (!Slave_Wait_Idle(SLAVE_ADDRESS(i)))
            return;
    }

    // 标记清洗状态
    rinse_state |= mask;

    // 打开注射器阀门
    for(i = 1; i < MAX_SLAVE_COUNT; ++i)
    {
        if ((mask & (1 << i)) == 0)
            continue;

        Uart2_Begin_Write_Word(SLAVE_ADDRESS(i), 0x09, 1);
        UART2_APPEND2(2, Z_ACTION_OPEN_VALVE);
        UART2_END();

        if (!Uart2_Exchange_Message())
        {
            Slave_Communication_Error(SLAVE_ADDRESS(i));
            return;
        }
    }

    // 等待Z移动完成
    for(i = 1; i < MAX_SLAVE_COUNT; ++i)
    {
        if ((mask & (1 << i)) == 0)
            continue;

        if (!Slave_Wait_Idle(SLAVE_ADDRESS(i)))
            return;
    }

    // 启动蠕动泵
    for(i = 1; i < MAX_SLAVE_COUNT; ++i)
    {
        if ((mask & (1 << i)) == 0)
            continue;

        Uart2_Begin_Write_Word(PUMP_ADDRESS(i), 0x05, 2);
        UART2_APPEND2(2, 1);
        UART2_APPEND2(2, rw_word.REAGENT_VOLUME);
        UART2_END();
        if (!Uart2_Exchange_Message())
        {
            Slave_Communication_Error(PUMP_ADDRESS(i));
            return;
        }

        // 只有一个清洗泵, 发一次命令就可以了
        if (!rw_bit.independent_rinse)
            break;
    }

    // 等待清洗完成
    for(i = 1; i < MAX_SLAVE_COUNT; ++i)
    {
        if ((mask & (1 << i)) == 0)
            continue;

        if (!Slave_Wait_Idle(PUMP_ADDRESS(i)))
            return;

        // 只有一个清洗泵
        if (!rw_bit.independent_rinse)
            break;
    }

    // 读取清洗板报警状态
    for(i = 1; i < MAX_SLAVE_COUNT; ++i)
    {
        if ((mask & (1 << i)) == 0)
            continue;

        if (Uart2_Read_RoBit(PUMP_ADDRESS(i), 0, 4))
            Write_RO_bit(i*4, uart2_rx_buf[3], 4);
    }

    // 等待1秒后, 离开清洗状态
    here_time = 0;
    here_time = SystemTickHigh();
    while((SystemTickHigh() - here_time) < 1000)
            Main_Loop();
        

    // 注射器回到原点, 关闭阀门
    for(i = 1; i < MAX_SLAVE_COUNT; ++i)
    {
        if ((mask & (1 << i)) == 0)
            continue;

        Uart2_Begin_Write_Word(SLAVE_ADDRESS(i), 0x09, 4);
        UART2_APPEND2(2, Z_ACTION_S_MOVE_TO);
        UART2_APPEND2(4,   0);
        UART2_APPEND2(3,  0);
        UART2_END();

        if (!Uart2_Exchange_Message())
        {
            Slave_Communication_Error(SLAVE_ADDRESS(i));
            return;
        }
    }

    // 等待Z完成
    for(i = 1; i < MAX_SLAVE_COUNT; ++i)
    {
        if ((mask & (1 << i)) == 0)
            continue;

        if (!Slave_Wait_Idle(SLAVE_ADDRESS(i)))
            return;
    }

    no_pump_reversal = (rw_word.REAGENT_BOTTLE_TYPE & REAGENT_FLAG_NO_REVERSAL) ? 1 : 0;

    // 清洗完后抬起需要抬起的针
    if (!(rw_word.REAGENT_BOTTLE_TYPE & REAGENT_FLAG_HOLD_POSITION))
    {
        Do_Action_Leave_Rinse_State();
    }
}

#define MEASURE_MICROSTEP_2         0   // 2 细分定位
#define MEASURE_MICROSTEP_8         2   // 8 细分定位
#define MEASURE_MICROSTEP_16        3   // 16细分定位
#define MEASURE_MICROSTEP_32        4   // 32细分定位
#define MEASURE_MICROSTEP_64        5   // 16细分定位

#ifdef ENABLE_MEASURE_MICROSTEP
    #define MEASURE_MICROSTEP_CODE      MEASURE_MICROSTEP_16
#else
    #define MEASURE_MICROSTEP_CODE      MEASURE_MICROSTEP_16
#endif//ENABLE_MEASURE_MICROSTEP

#if MEASURE_MICROSTEP_CODE == MEASURE_MICROSTEP_64
    #define X_MOTOR_EXCITATION      X_MOTOR_EXCITATION_64
    #define Y_MOTOR_EXCITATION      Y_MOTOR_EXCITATION_64
#elif MEASURE_MICROSTEP_CODE == MEASURE_MICROSTEP_32
    #define X_MOTOR_EXCITATION      X_MOTOR_EXCITATION_32
    #define Y_MOTOR_EXCITATION      Y_MOTOR_EXCITATION_32
#elif MEASURE_MICROSTEP_CODE == MEASURE_MICROSTEP_16
    #define X_MOTOR_EXCITATION      X_MOTOR_EXCITATION_16
    #define Y_MOTOR_EXCITATION      Y_MOTOR_EXCITATION_16
#elif MEASURE_MICROSTEP_CODE == MEASURE_MICROSTEP_8
    #define X_MOTOR_EXCITATION      X_MOTOR_EXCITATION_8
    #define Y_MOTOR_EXCITATION      Y_MOTOR_EXCITATION_8
#else
    #undef  MEASURE_MICROSTEP_CODE
    #define MEASURE_MICROSTEP_CODE  MEASURE_MICROSTEP_64
    #define X_MOTOR_EXCITATION      X_MOTOR_EXCITATION_64
    #define Y_MOTOR_EXCITATION      Y_MOTOR_EXCITATION_64
#endif//MEASURE_MICROSTEP_CODE


// 测量次数, 至少2次
#define MEASURE_REPEAT              2
// X测量时的速度, 0.01 rad/sec
#define X_MEASURE_SPEED             ((ushort)(MEASURE_SPEED * 2 * 3.14159 * 100 / X_PERIMETER * (1 << MEASURE_MICROSTEP_CODE)))

// X测量时的速度, 0.01 rad/sec
#define Y_MEASURE_SPEED             ((ushort)(MEASURE_SPEED * 2 * 3.14159 * 100 / Y_PERIMETER * (1 << MEASURE_MICROSTEP_CODE)))

// 测试高度时, X距边的步数
#define X_MEASURE_OFFSET            ((ushort)(MEASURE_OFFSET * SPR / X_PERIMETER * (1 << MEASURE_MICROSTEP_CODE)))

// 测试高度时, Y距边的步数
#define Y_MEASURE_OFFSET            ((ushort)(MEASURE_OFFSET * SPR / Y_PERIMETER * (1 << MEASURE_MICROSTEP_CODE)))

#define SIDE_TOP                    0
#define SIDE_RIGHT                  1
#define SIDE_BOTTOM                 2
#define SIDE_LEFT                   3
#define SIDE_CENTER                 4
#define SIDE_NONE                   5

#define MEASURE_TOP                 (1 << SIDE_TOP)
#define MEASURE_RIGHT               (1 << SIDE_RIGHT)
#define MEASURE_BOTTOM              (1 << SIDE_BOTTOM)
#define MEASURE_LEFT                (1 << SIDE_LEFT)

#define MEASURE_HEIGHT_MASK         (0xF0)
#define MEASURE_HEIGHT_AT_TOP       (SIDE_TOP << 4)
#define MEASURE_HEIGHT_AT_RIGHT     (SIDE_RIGHT << 4)
#define MEASURE_HEIGHT_AT_BOTTOM    (SIDE_BOTTOM << 4)
#define MEASURE_HEIGHT_AT_LEFT      (SIDE_LEFT << 4)
#define MEASUER_HEIGHT_AT_CENTER    (SIDE_CENTER << 4)
#define MEASUER_HEIGHT_AT_NONE      (SIDE_NONE << 4)

#if defined(T0_RELOAD)
#define T0_MEASURE_BASE             (36)
#define T0_MEASURE_RELOAD_Y         (256 - T0_MEASURE_BASE)
#define T0_MEASURE_RELOAD_X         (256 - X_PERIMETER / Y_PERIMETER * T0_MEASURE_BASE)
#endif


void Do_Measure(uchar slave)
{
    long pos[4];    // SIDE_xxx
    uchar i, j;
    uchar robit_index = SLAVE_ADDRESS(0) == slave ? 5 : 2;

    //
    // WARNING: Z0使用反射光耦, 应当在表面以上的反射距离内
    //          Z1/Z2应当在表面以下一定距离内, 以便接触到表面
    //

#ifndef IGNORE_Z
    // 检测Z轴高度
    Uart2_Begin_Write_Word(slave, 0x09, 5);
    UART2_APPEND2(2, Z_ACTION_PROBE_AND_ASPIRATE);
    UART2_APPEND2(4, rw_word.Z_TARGET_1);
    UART2_APPEND2(2, 0);
    UART2_APPEND2(2, REAGENT_FLAG_HOLD_POSITION);
    UART2_END();
    if (!Uart2_Exchange_Message())
    {
        Slave_Communication_Error(slave);
        return;
    }

    // 等待完成
    if (!Slave_Wait_Idle(slave))
    {
        // 如果是通讯错误, 结束; 其它错误只有可能是未检测到液面或未检测到测试杯传感器, 忽略
        if ((uchar)server_status == XY_STATUS_COMM_ERROR)
            return;
    }

    // 此处理应检测不到液面, 因为要探测的位置下面是空的
    // 如果检测到液面, 报错
    if (server_status == STATUS_IDLE)
    {
        server_status = XY_STATUS_MEASURE_ERROR;
        return;
    }
#endif//IGNORE_Z

    for(i = 0; i < MEASURE_REPEAT; ++i)
    {
        //
        // X方向左右运行, 检测Z1/2液面传感器值或Z0反射光耦值
        //

        for(j = 0; j < 2; ++j)
        {
            if ((j == 0 && (rw_word.REAGENT_VOLUME & MEASURE_LEFT) == 0)
                    || (j == 1 && (rw_word.REAGENT_VOLUME & MEASURE_RIGHT) == 0))
                continue;

#ifndef IGNORE_Z
            // 重置液面检测
            if (slave != SLAVE_ADDRESS(0))
            {
                Uart2_Begin_Write_Bit(slave, 0, 1);
                UART2_APPEND2(1, 1);
                UART2_END();
                if (!Uart2_Exchange_Message())
                {
                    Slave_Communication_Error(slave);
                    return;
                }
            }
#endif// IGNORE_Z

            server_status = XY_STATUS_MEASURE_ERROR;

#if defined(T0_RELOAD)
            TH0 = T0_MEASURE_RELOAD_X;
#endif

            // 启动X电机, x_target_2为X方向的最大距离
            X_Init_Move(1);
            speed_cntr_Move(&srd_x, j ? rw_word.X_TARGET_2 : -rw_word.X_TARGET_2, X_MEASURE_SPEED);

            // 等待X结束或检测到液面传感器变化
            while(srd_x.running)
            {
                Process_Main_Loop();

#ifndef IGNORE_Z
                // 读取液面检测状态, 为1时停止
                if (!Uart2_Read_RoBit(slave, robit_index, 1))
                {
                    Slave_Communication_Error(slave);
                    return;
                }

                // 检测到液面, 停止电机
                if (uart2_rx_buf[3] & 1)
                {
                    X_Stop_Timer();
                    Stop_X_Motor();
                    server_status = STATUS_IDLE;
                    break;
                }
#endif// IGNORE_Z
            }

#if defined(T0_RELOAD)
            TH0 = T0_RELOAD;
#endif

#ifndef IGNORE_Z
            // 未检测到变化, 定位失败
            if (server_status != STATUS_IDLE)
                return;
#endif// IGNORE_Z

            if (j == 0)
            {
                pos[SIDE_LEFT] = ro_word.X_POS;
                if ((rw_word.REAGENT_VOLUME & MEASURE_RIGHT) == 0)
                    rw_word.X_TARGET_1 = ro_word.X_POS + (rw_word.X_TARGET_2 >> 1);
            }
            else
            {
                pos[SIDE_RIGHT] = ro_word.X_POS;

                // 计算X中心坐标和宽度
                if (rw_word.REAGENT_VOLUME & MEASURE_LEFT)
                    rw_word.X_TARGET_1 = (pos[SIDE_RIGHT] + pos[SIDE_LEFT]) >> 1; // x / 2
                else
                    rw_word.X_TARGET_1 = ro_word.X_POS - (rw_word.X_TARGET_2 >> 1);
            }

            // 回到中心位置
            X_Init_Move(0);
            speed_cntr_Move(&srd_x, rw_word.X_TARGET_1 - ro_word.X_POS, rw_word.XMAX_SPEED);
            while(srd_x.running)
                Process_Main_Loop();
        }

        //
        // Y方向左右运行, 同X
        //

        for(j = 0; j < 2; ++j)
        {
            if ((j == 0 && (rw_word.REAGENT_VOLUME & MEASURE_TOP) == 0)
                    || (j == 1 && (rw_word.REAGENT_VOLUME & MEASURE_BOTTOM) == 0))
                continue;

#ifndef IGNORE_Z
            // 重置液面检测
            if (slave != SLAVE_ADDRESS(0))
            {
                Uart2_Begin_Write_Bit(slave, 0, 1);
                UART2_APPEND2(1, 1);
                UART2_END();
                if (!Uart2_Exchange_Message())
                {
                    Slave_Communication_Error(slave);
                    return;
                }
            }
#endif//IGNORE_Z

            server_status = XY_STATUS_MEASURE_ERROR;

#if defined(T0_RELOAD)
            TH0 = T0_MEASURE_RELOAD_Y;
#endif

            // 启动Y电机, reagent_bottle_type为Y方向的最大距离
            Y_Init_Move(1);
            speed_cntr_Move(&srd_y, j ? rw_word.Y_TARGET_2 : -rw_word.Y_TARGET_2, Y_MEASURE_SPEED);

            // 等待Y结束或检测到液面传感器变化
            while(srd_y.running)
            {
                Process_Main_Loop();

#ifndef IGNORE_Z
                // 读取液面检测状态, 为1时停止
                if (!Uart2_Read_RoBit(slave, robit_index, 1))
                {
                    Slave_Communication_Error(slave);
                    return;
                }

                // 检测到液面, 停止电机
                if (uart2_rx_buf[3] & 1)
                {
                    Y_Stop_Timer();
                    Stop_Y_Motor();
                    server_status = STATUS_IDLE;
                    break;
                }
#endif// IGNORE_Z
            }

#if defined(T0_RELOAD)
            TH0 = T0_RELOAD;
#endif

#ifndef IGNORE_Z
            // 未检测到变化, 定位失败
            if (server_status != STATUS_IDLE)
                return;
#endif// IGNORE_Z

            if (j == 0)
            {
                pos[SIDE_TOP] = ro_word.Y_POS;
                if ((rw_word.REAGENT_VOLUME & MEASURE_BOTTOM) == 0)
                    rw_word.Y_TARGET_1 = ro_word.Y_POS + (rw_word.Y_TARGET_2 >> 1);
            }
            else
            {
                pos[SIDE_BOTTOM] = ro_word.Y_POS;

                // 计算X中心坐标和宽度
                if (rw_word.REAGENT_VOLUME & MEASURE_TOP)
                    rw_word.Y_TARGET_1 = (pos[SIDE_BOTTOM] + pos[SIDE_TOP]) >> 1; // x / 2
                else
                    rw_word.Y_TARGET_1 = ro_word.Y_POS - (rw_word.Y_TARGET_2 >> 1);
            }

            // 回到中心位置
            Y_Init_Move(0);
            speed_cntr_Move(&srd_y, rw_word.Y_TARGET_1 - ro_word.Y_POS, rw_word.YMAX_SPEED);
            while(srd_y.running)
                Process_Main_Loop();
        }
    }

    // 保存X的测量位置
    if ((rw_word.REAGENT_VOLUME & (MEASURE_LEFT | MEASURE_RIGHT)) == (MEASURE_LEFT | MEASURE_RIGHT))
        rw_word.X_TARGET_1 = (pos[SIDE_LEFT] + pos[SIDE_RIGHT]) >> 1;
    else if (rw_word.REAGENT_VOLUME & MEASURE_LEFT)
    {
        pos[SIDE_RIGHT] = pos[SIDE_LEFT] + rw_word.X_TARGET_2;
        rw_word.X_TARGET_1 = pos[SIDE_LEFT];
    }
    else if (rw_word.REAGENT_VOLUME & MEASURE_RIGHT)
    {
        pos[SIDE_LEFT] = pos[SIDE_RIGHT] - rw_word.X_TARGET_2;
        rw_word.X_TARGET_1 = pos[SIDE_RIGHT];
    }

    // 保存Y的测量位置
    if ((rw_word.REAGENT_VOLUME & (MEASURE_TOP | MEASURE_BOTTOM)) == (MEASURE_TOP | MEASURE_BOTTOM))
        rw_word.Y_TARGET_1 = (pos[SIDE_TOP] + pos[SIDE_BOTTOM]) >> 1;
    else if (rw_word.REAGENT_VOLUME & MEASURE_TOP)
    {
        pos[SIDE_BOTTOM] = pos[SIDE_TOP] + rw_word.Y_TARGET_2;
        rw_word.Y_TARGET_1 = pos[SIDE_TOP];
    }
    else if (rw_word.REAGENT_VOLUME & MEASURE_BOTTOM)
    {
        pos[SIDE_TOP] = pos[SIDE_BOTTOM] - rw_word.Y_TARGET_2;
        rw_word.Y_TARGET_1 = pos[SIDE_BOTTOM];
    }

    //
    // XY坐标测量完成, 复位Z, 测量Z的高度
    //

#ifndef IGNORE_Z
    // 所有Z轴回零位
    if (!Slave_Home())
        return;

    // 机械手始终在中心位置探测高度
    if (slave == SLAVE_ADDRESS(0)
            && (rw_word.REAGENT_VOLUME & MEASURE_HEIGHT_MASK) != MEASUER_HEIGHT_AT_NONE)
    {
        rw_word.REAGENT_VOLUME &= ~MEASURE_HEIGHT_MASK;
        rw_word.REAGENT_VOLUME |= MEASUER_HEIGHT_AT_CENTER;
    }

    // 将XY移到边上
    switch(rw_word.REAGENT_VOLUME & MEASURE_HEIGHT_MASK)
    {
    case MEASURE_HEIGHT_AT_TOP:
        if (!Move_XY_To(rw_word.X_TARGET_1, pos[SIDE_TOP] - Y_MEASURE_OFFSET, rw_word.XMAX_SPEED, rw_word.YMAX_SPEED))
            return;
        break;

    case MEASURE_HEIGHT_AT_BOTTOM:
        if (!Move_XY_To(rw_word.X_TARGET_1, pos[SIDE_BOTTOM] + Y_MEASURE_OFFSET, rw_word.XMAX_SPEED, rw_word.YMAX_SPEED))
            return;
        break;

    case MEASURE_HEIGHT_AT_LEFT:
        if (!Move_XY_To(pos[SIDE_LEFT] - X_MEASURE_OFFSET, rw_word.Y_TARGET_1, rw_word.XMAX_SPEED, rw_word.YMAX_SPEED))
            return;
        break;

    case MEASURE_HEIGHT_AT_RIGHT:
        if (!Move_XY_To(pos[SIDE_RIGHT] + X_MEASURE_OFFSET, rw_word.Y_TARGET_1, rw_word.XMAX_SPEED, rw_word.YMAX_SPEED))
            return;
        break;

    case MEASUER_HEIGHT_AT_CENTER:
        if (!Move_XY_To((pos[SIDE_LEFT] + pos[SIDE_RIGHT]) >> 1, (pos[SIDE_TOP] + pos[SIDE_BOTTOM]) >> 1, rw_word.XMAX_SPEED, rw_word.YMAX_SPEED))
            return;
        break;

    default:
        break;
    }

    // 探测Z高度
    if ((rw_word.REAGENT_VOLUME & MEASURE_HEIGHT_MASK) != MEASUER_HEIGHT_AT_NONE)
    {
        for(j=0; j<2; ++j)
        {
            // 检测Z轴高度
            Uart2_Begin_Write_Word(slave, 0x09, 5);
            UART2_APPEND2(2, slave == SLAVE_ADDRESS(0) ? Z_ACTION_PICK_CUP : Z_ACTION_PROBE_AND_ASPIRATE);
            UART2_APPEND2(4,   ((rw_word.REAGENT_VOLUME & MEASURE_HEIGHT_MASK) == MEASUER_HEIGHT_AT_CENTER
                                  || slave == SLAVE_ADDRESS(0)) ? rw_word.Z_TARGET_1 + rw_word.Z_TARGET_2 : rw_word.Z_TARGET_1);
            UART2_APPEND2(2, 0);
            UART2_APPEND2(2, 0);
            UART2_END();
            if (!Uart2_Exchange_Message())
            {
                Slave_Communication_Error(slave);
                return;
            }

            // 等待完成
            if (!Slave_Wait_Idle(slave))
                return;

#ifdef IGNORE_PICKUP_ERROR
            // 忽略抓杯传感器错误
            if (slave == SLAVE_ADDRESS(0) && Z_STATUS_PICK_CUP_SENSOR_ERROR == *(ushort*)&uart1_rx_buf[2])
            {
                Slave_Status_Error(slave, *(ushort*)&uart1_rx_buf[2]);
                return;
            }
#endif//IGNORE_PICKUP_ERROR

            // 读取液面位置
            if (!Uart2_Read_RoWord(slave, slave == SLAVE_ADDRESS(0) ? 0x05 : 0x09, 2))
            {
                Slave_Communication_Error(slave);
                return;
            }

            pos[j] = *(long*)&uart2_rx_buf[3];

            if (!Slave_Z_Home(slave) || !Slave_Wait_Idle(slave))
                return;
        }

        rw_word.Z_TARGET_1 = (pos[0] + pos[1]) >> 1;
    }
#endif// IGNORE_Z
}


void Do_Action_Disable_Motor()
{
    uchar i;

    Spi0_ms35711X_write(0x00,Ctrl_register& (~0x0001));
    Spi0_ms35711Y_write(0x00,Ctrl_register& (~0x0001));
    rinse_state = 0;    // 清除清洗状态

    for(i=0; i<MAX_SLAVE_COUNT; ++i)
    {
        if ((*(char*)slave_flag & (1 << i)) == 0)
            continue;

        Uart2_Begin_Write_Word(SLAVE_ADDRESS(i), 0x09, 1);
        UART2_APPEND2(2, Z_ACTION_DISABLE_Z_MOTOR);
        UART2_END();
        if (!Uart2_Exchange_Message())
            Slave_Communication_Error(SLAVE_ADDRESS(i));
    }
}



void Do_Action_Dispense_And_Mix(uchar slave)
{
    uchar times = rw_word.REAGENT_BOTTLE_TYPE >> 8; // 混匀次数
    // NOTE: 正常情况下, 执行此运作时应当已经离开了清洗状态
    // 离开清洗状态
    if (!Do_Action_Leave_Rinse_State())
        return;
 
    // 移动到指定位置
    if (!Move_XYZ_To(slave))
        return;

    rw_word.REAGENT_BOTTLE_TYPE &= 0xFF;
    rw_word.REAGENT_BOTTLE_TYPE |= REAGENT_FLAG_IGNORE_PROBE_ERROR | REAGENT_FLAG_NO_DELAY;
    // 如果需要混匀, 加样完后不要把针抬起来
    if (times)
        rw_word.REAGENT_BOTTLE_TYPE |= REAGENT_FLAG_HOLD_POSITION;

    // 加样
    Uart2_Begin_Write_Word(slave, 0x09, 5);
    UART2_APPEND2(2, Z_ACTION_ASPIRATE);
    UART2_APPEND2(4,   rw_word.Z_TARGET_1);
    UART2_APPEND2(3,  -rw_word.REAGENT_VOLUME);
    UART2_APPEND2(2, rw_word.REAGENT_BOTTLE_TYPE);
    UART2_END();

    if (!Uart2_Exchange_Message())
    {
        Slave_Communication_Error(slave);
        return;
    }

    if (!Slave_Wait_Idle(slave))
        return;

    // 混匀至少100ul
    if (rw_word.REAGENT_VOLUME < 10000)
        rw_word.REAGENT_VOLUME = 10000;
    times <<= 1;
    while(times--)
    {
        // 最后一次完成后将针抬起
        if (times == 0)
            rw_word.REAGENT_BOTTLE_TYPE &= ~REAGENT_FLAG_HOLD_POSITION;

        // 加样或吸样
        Uart2_Begin_Write_Word(slave, 0x09, 5);
        UART2_APPEND2(2, Z_ACTION_MOVE_AND_ASPIRATE);
        UART2_APPEND2(4,   (times & 1) ? rw_word.Z_TARGET_2 : rw_word.Z_TARGET_1);
        UART2_APPEND2(3,  rw_word.REAGENT_VOLUME);
        UART2_APPEND2(2, rw_word.REAGENT_BOTTLE_TYPE);
        UART2_END();

        if (!Uart2_Exchange_Message())
        {
            Slave_Communication_Error(slave);
            return;
        }

        if (!Slave_Wait_Idle(slave))
            return;

        rw_word.REAGENT_VOLUME = -rw_word.REAGENT_VOLUME;
    }
}

void Do_Action_Move_XYZ_To(uchar slave)
{
    // 离开清洗状态
    if (!Do_Action_Leave_Rinse_State())
        return;

    Move_XYZ_To(slave);
}

void Do_Action_Home(bit homeX, bit homeY)
{
    uchar i;

    // 离开清洗状态
    if (!Do_Action_Leave_Rinse_State())
        return;

    if ((!homeX || X_AT_HOME) && (!homeY || Y_AT_HOME))
        return;

    // 发送复位Z指令
    for(i=0; i<MAX_SLAVE_COUNT; ++i)
    {
        if (!Z_VALID_INDEX(i))
            continue;

        Uart2_Begin_Write_Word(SLAVE_ADDRESS(i), 0x09, 1);
        UART2_APPEND2(2, Z_ACTION_Z_RESET);
        UART2_END();

        if (!Uart2_Exchange_Message())
        {
            Slave_Communication_Error(SLAVE_ADDRESS(i));
            return;
        }
    }

    // 等待Z复位完成
    for(i=0; i<MAX_SLAVE_COUNT; ++i)
    {
        if (!Slave_Wait_Idle(SLAVE_ADDRESS(i)))
            return;
    }

    server_status = STATUS_IDLE;

    // 复位Y轴
    if (homeY && !Y_AT_HOME)
    {
        Y_Init_Reset();
        srd_y.target = srd_y.home_state ? Y_RESET_BUFFER_STEP : -Y_RESET_BUFFER_STEP;
        speed_cntr_Move(&srd_y, srd_y.home_state ? rw_word.Y_MAX_DISTANCE : -rw_word.Y_MAX_DISTANCE, rw_word.YMAX_SPEED);

        // 等待Y轴完成
        while(srd_y.running)
            Process_Main_Loop();

        // 判断Y复位状态
        if (server_status != STATUS_IDLE)
            return;
    }

    // 复位X轴
    if (homeX && !X_AT_HOME)
    {
        X_Init_Reset();
        srd_x.target = srd_x.home_state ? X_RESET_BUFFER_STEP : -X_RESET_BUFFER_STEP;
        speed_cntr_Move(&srd_x, srd_x.home_state ? rw_word.X_MAX_DISTANCE : -rw_word.X_MAX_DISTANCE, rw_word.XMAX_SPEED);

        // 等待X轴完成
        while(srd_x.running)
            Process_Main_Loop();

        // 判断X复位状态
        if (server_status != STATUS_IDLE)
            return;
    }

    server_status = STATUS_IDLE;
}






void Process_Action()
{
    ps_server_busy = 1;
    switch (rw_word.ACTION)
    {
    case XY_ACTION_STOP:
        Do_Action_Stop();
        break;

    case XY_ACTION_RESET:
        server_status = STATUS_IDLE;
        Do_Action_Reset();
        break;

    case XY_ACTION_PICK_CUP:
    case XY_ACTION_RELEASE_CUP:
        server_status = STATUS_IDLE;
        Do_Action_Pick_Cup(XY_ACTION_PICK_CUP == rw_word.ACTION);
        break;

    case XY_ACTION_ASPIRATE_1:
    case XY_ACTION_ASPIRATE_2:
        server_status = STATUS_IDLE;
        Do_Action_Aspirate(SLAVE_ADDRESS(rw_word.ACTION - XY_ACTION_ASPIRATE_1 + 1), 0);
        break;

    case XY_ACTION_DISPENSE_1:
    case XY_ACTION_DISPENSE_2:
        server_status = STATUS_IDLE;
        rw_word.REAGENT_VOLUME = -rw_word.REAGENT_VOLUME;
        Do_Action_Aspirate(SLAVE_ADDRESS(rw_word.ACTION - XY_ACTION_DISPENSE_1 + 1), 0);
        break;

    case XY_ACTION_RINSE_1:
    case XY_ACTION_RINSE_2:
    case XY_ACTION_RINSE_ALL:
        server_status = STATUS_IDLE;
        Do_Action_Rinse(rw_word.ACTION == XY_ACTION_RINSE_1 ? 0x02 :
                        rw_word.ACTION == XY_ACTION_RINSE_2 ? 0x04 : 0x06);
        break;

    case XY_ACTION_LEAVE_RINSE_STATE:
        server_status = STATUS_IDLE;
        Do_Action_Leave_Rinse_State();
        break;

    case XY_ACTION_TRANSFER_CUP:
        server_status = STATUS_IDLE;
        Do_Action_Pick_Cup(1);
        if (server_status == STATUS_IDLE)
        {
            rw_word.X_TARGET_1 = rw_word.X_TARGET_2;
            rw_word.Y_TARGET_1 = rw_word.Y_TARGET_2;
            rw_word.Z_TARGET_1 = rw_word.Z_TARGET_2;
            Do_Action_Pick_Cup(0);
        }
        break;

    case XY_ACTION_TRANSFER_REAGENT_1:
    case XY_ACTION_TRANSFER_REAGENT_2:
        server_status = STATUS_IDLE;
        Do_Action_Aspirate(SLAVE_ADDRESS(rw_word.ACTION - XY_ACTION_TRANSFER_REAGENT_1 + 1), 0);
        if (server_status == STATUS_IDLE)
        {
            rw_word.X_TARGET_1 = rw_word.X_TARGET_2;
            rw_word.Y_TARGET_1 = rw_word.Y_TARGET_2;
            rw_word.Z_TARGET_1 = rw_word.Z_TARGET_2;
            rw_word.REAGENT_VOLUME = -rw_word.REAGENT_VOLUME;
            Do_Action_Aspirate(SLAVE_ADDRESS(rw_word.ACTION - XY_ACTION_TRANSFER_REAGENT_1 + 1), 1);
        }
        break;

//    case XY_ACTION_MEASURE_0:
//    case XY_ACTION_MEASURE_1:
//    case XY_ACTION_MEASURE_2:
//        server_status = STATUS_IDLE;
//        Do_Action_Measure(SLAVE_ADDRESS(action - XY_ACTION_MEASURE_0));
//        break;

    case XY_ACTION_PROBE_LEVEL_0:
    case XY_ACTION_PROBE_LEVEL_1:
    case XY_ACTION_PROBE_LEVEL_2:
        server_status = STATUS_IDLE;
        rw_word.REAGENT_VOLUME = 0;
        Do_Action_Aspirate(SLAVE_ADDRESS(rw_word.ACTION - XY_ACTION_PROBE_LEVEL_0), 0);
        break;

    case XY_ACTION_MOVE_XY:
        server_status = STATUS_IDLE;
        Move_XY_To((rw_word.X_TARGET_1), rw_word.Y_TARGET_1, rw_word.XMAX_SPEED, rw_word.YMAX_SPEED);
        break;

    case XY_ACTION_DISABLE_MOTOR:
        server_status = STATUS_IDLE;
        Do_Action_Disable_Motor();
        break;

    case XY_ACTION_DISPENSE_AND_MIX_1:
    case XY_ACTION_DISPENSE_AND_MIX_2:
        server_status = STATUS_IDLE;
        Do_Action_Dispense_And_Mix(SLAVE_ADDRESS(rw_word.ACTION - XY_ACTION_DISPENSE_AND_MIX_1 + 1));
        break;

    case XY_ACTION_MOVE_XYZ_0:
    case XY_ACTION_MOVE_XYZ_1:
    case XY_ACTION_MOVE_XYZ_2:
        server_status = STATUS_IDLE;
        Do_Action_Move_XYZ_To(SLAVE_ADDRESS(rw_word.ACTION - XY_ACTION_MOVE_XYZ_0));
        break;

    case XY_ACTION_HOME_YX:
        server_status = STATUS_IDLE;
        Do_Action_Home(1, 1);
        break;

    case XY_ACTION_HOME_X:
        server_status = STATUS_IDLE;
        Do_Action_Home(1, 0);
        break;
    default:
        break;
    }
    // 没有新的动作请求，清除忙
    if(!pending_action)
        ps_server_busy = 0;
}
